/*******************************************************************************
* Copyright (c) 2008, Jeferson Jose de Miranda                                 *
* UDESC - Universidade do Estado de Santa Catarina                             *
* All rights reserved.                                                         *
*                                                                              *
* Redistribution and use in source and binary forms, with or without           *
* modification, are permitted provided that the following conditions are met:  *
*     * Redistributions of source code must retain the above copyright         *
*       notice, this list of conditions and the following disclaimer.          *
*     * Redistributions in binary form must reproduce the above copyright      *
*       notice, this list of conditions and the following disclaimer in the    *
*       documentation and/or other materials provided with the distribution.   *
*     * Neither the name of the Universidade do Estado de Santa Catarina nor   *
*       the names of its contributors may be used to endorse or promote        *
*       products derived from this software without specific prior written     *
*       permission.                                                            *
*                                                                              *
* THIS SOFTWARE IS PROVIDED BY JEFERSON JOSE DE MIRANDA "AS IS" AND ANY        *
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED    *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE       *
* DISCLAIMED. IN NO EVENT SHALL JEFERSON JOSE DE MIRANDA BE LIABLE FOR ANY     *
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES   *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND  *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT   *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF     *
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.            *
*******************************************************************************/

/**
 * \file ExtremePoints.hpp
 *
 * \brief This is an array of 2D points classified as convex or concave.
 *
 * \author Jeferson Jose de Miranda
 */

#ifndef EXTREMEPOINTS_HPP
#define EXTREMEPOINTS_HPP

/**
* \def CONVEX
* Conex type of point.
*/
#define CONVEX true

/**
* \def CONCAVE
* Concave type of point.
*/
#define CONCAVE false

// OpenCV main header.
#include <cv.h>
// A sequence of 2D ponts.
#include "PointSequence.hpp"
// Groups Move-In exception messages.
#include "Exceptions.hpp"

namespace MoveIn
{
    /**
    * \class ExtremePoints
    *
    * \brief This is an array of 2D points classified as convex or concave.
    *
    * This class is composed by two OpenCV Sequences. A CvPoint Sequence, that
    * stores the pixel coordinates, and a boolean sequen
    */
    class ExtremePoints : public PointSequence
    {
        private:
        CvSeq *curvature_sequence;

        CvMemStorage *curvature_storage;

        unsigned int euclideanDistance(CvPoint p1,
                                       CvPoint p2);

        CvPoint ExtremePoints::midPoint(CvPoint p1,
                                        CvPoint p2);

        public:
        ExtremePoints();

        ~ExtremePoints();

        void append(CvPoint p,
                    bool curvature);

        void insert(CvPoint p,
                    bool curvature,
                    unsigned int index);

        void remove(unsigned int index);

        bool getCurvature(unsigned int index);

        bool getFirstCurvature();

        bool getLastCurvature();

        void reset();

        void reduceConvex(unsigned int distance);

        void reduceConcave(unsigned int distance);

        float dotProduct(unsigned int index);

        float angle(unsigned int index);

        int boundaryPoint(unsigned char boundary,
                          bool curvature);

        void reorder(unsigned char boundary);
    };

/*******************************************************************************
* MoveIn::ExtremePoints::ExtremePoints                                         *
*******************************************************************************/

    ExtremePoints::ExtremePoints() : PointSequence()
    {
        curvature_storage = NULL;
        curvature_storage = cvCreateMemStorage(0);
        if(curvature_storage == NULL)
        {
            throw new OutOfMemory();
        }
        else
        {
            curvature_sequence = cvCreateSeq(0,
                                             sizeof(*curvature_sequence),
                                             sizeof(bool),
                                             curvature_storage);
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::~ExtremePoints                                        *
*******************************************************************************/

    ExtremePoints::~ExtremePoints()
    {
        cvClearSeq(curvature_sequence);
        cvReleaseMemStorage(&curvature_storage);
    }

/*******************************************************************************
* MoveIn::ExtremePoints::append                                                *
*******************************************************************************/

    void ExtremePoints::append(CvPoint p,
                               bool curvature)
    {
        PointSequence::append(p);
        cvSeqPush(curvature_sequence, &curvature);
    }


/*******************************************************************************
* MoveIn::ExtremePoints::insert                                                *
*******************************************************************************/

    void ExtremePoints::insert(CvPoint p,
                               bool curvature,
                               unsigned int index)
    {
        if(index > (unsigned int) curvature_sequence->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            PointSequence::insert(p, index);
            cvSeqInsert(curvature_sequence, index, &curvature);
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::remove                                                *
*******************************************************************************/

    void ExtremePoints::remove(unsigned int index)
    {
        if(index >= (unsigned int) curvature_sequence->total)
        {
            throw new OutOfBounds();
        }
        else
        {
            PointSequence::remove(index);
            cvSeqRemove(curvature_sequence, index);
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::getCurvature                                          *
*******************************************************************************/

    bool ExtremePoints::getCurvature(unsigned int index)
    {
        try
        {
            int idx = realIndex(index);
            bool *c = (bool*) cvGetSeqElem(curvature_sequence, idx);
            return *c;
        }
        catch(std::exception *e)
        {
            throw e;
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::getFirstCurvature                                     *
*******************************************************************************/

    bool ExtremePoints::getFirstCurvature()
    {
        return getCurvature(0);
    }

/*******************************************************************************
* MoveIn::ExtremePoints::getLastCurvature                                      *
*******************************************************************************/

    bool ExtremePoints::getLastCurvature()
    {
        if(curvature_sequence->total == 0)
        {
            throw new OutOfBounds();
        }
        else
        {
            return getCurvature(curvature_sequence->total - 1);
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::reset                                                 *
*******************************************************************************/

    void ExtremePoints::reset()
    {
        PointSequence::reset();
        cvClearSeq(curvature_sequence);
    }

/*******************************************************************************
* MoveIn::ExtremePoints::reduceConvex                                          *
*******************************************************************************/

    void ExtremePoints::reduceConvex(unsigned int distance)
    {
        bool reduced = true;
        CvPoint first;
        CvPoint second;
        while(reduced)
        {
            reduced = false;
            if(size() > 1)
            {
                first = getPoint(0);
                for(unsigned int i=1; i<size(); i++)
                {
                    second = getPoint(i);
                    if(getCurvature(i-1) == CONVEX
                       &&
                       getCurvature(i)   == CONVEX)
                    {
                        if(euclideanDistance(first, second)
                           <
                           distance)
                        {
                            ExtremePoints::remove(i-1);
                            ExtremePoints::remove(i-1);
                            try
                            {
                                ExtremePoints::insert(midPoint(first, second),
                                                      CONVEX,
                                                      i-1);
                            }
                            catch(std::exception *e)
                            {
                                ExtremePoints::append(midPoint(first, second),
                                                      CONVEX);
                            }
                            reduced = true;
                            break;
                        }
                    }
                    first = second;
                }
            }
        }
        // Compare the first point with the last point.
        if(size() > 1)
        {
            if(getFirstCurvature() == CONVEX
               &&
               getLastCurvature() == CONVEX)
            {
                first = getPoint(0);
                second = getPoint(size()-1);
                ExtremePoints::remove(0);
                ExtremePoints::remove(size()-1);
                ExtremePoints::insert(midPoint(first, second),
                                      CONVEX,
                                      0);
            }
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::reduceConcave                                         *
*******************************************************************************/

    void ExtremePoints::reduceConcave(unsigned int distance)
    {
        bool reduced = true;
        CvPoint before;
        CvPoint after;
        CvPoint concave;
        int index;
        while(reduced)
        {
            reduced = false;
            if(size() > 1)
            {
                for(unsigned int i=0; i<size(); i++)
                {
                    concave = getPoint(i);
                    if(getCurvature(i) == CONCAVE)
                    {
                        index = (i==0) ? (size()-1) : (i-1);
                        before = getPoint(index);
                        index = (i==size()-1) ? (0) : (i+1);
                        after = getPoint(index);
                        if((euclideanDistance(before, concave)
                            +
                            euclideanDistance(concave, after))
                            <
                            distance)
                        {
                            ExtremePoints::remove(i);
                            reduced = true;
                            break;
                        }
                    }
                }
            }
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::euclideanDistance                                     *
*******************************************************************************/

    unsigned int ExtremePoints::euclideanDistance(CvPoint p1,
                                                  CvPoint p2)
    {
        unsigned int result;
        result = (unsigned int) sqrt(pow((p1.x-p2.x),2.0)
                                     +
                                     pow((p1.y-p2.y),2.0));
        return result;
    }

/*******************************************************************************
* MoveIn::ExtremePoints::midPoint                                              *
*******************************************************************************/

    CvPoint ExtremePoints::midPoint(CvPoint p1,
                                    CvPoint p2)
    {
        CvPoint result;
        result.x = (p1.x + p2.x) / 2;
        result.y = (p1.y + p2.y) / 2;
        return result;
    }

/*******************************************************************************
* MoveIn::ExtremePoints::dotProduct                                            *
*******************************************************************************/

    float ExtremePoints::dotProduct(unsigned int index)
    {
        if(index >= size())
        {
            throw new OutOfBounds();
        }
        else
        {
            CvPoint before, after, point;
            before = getPoint((index==0) ? (size()-1) : (index-1));
            after = getPoint((index==size()-1) ? (0) : (index+1));
            point = getPoint(index);
            // Bring vectors to origin.
            float bx, by, ax, ay;
            bx = before.x - point.x;
            by = before.y - point.y;
            ax = after.x - point.x;
            ay = after.y - point.y;
            // Normalize vectors.
            float magnitude;
            magnitude = sqrt(pow(bx, 2.0)
                           + pow(by, 2.0));
            bx /= magnitude;
            by /= magnitude;
            magnitude = sqrt(pow(ax, 2.0)
                           + pow(ay, 2.0));
            ax /= magnitude;
            ay /= magnitude;
            // Calcultate dot product.
            return bx*ax + by*ay;
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::angle                                                 *
*******************************************************************************/

    float ExtremePoints::angle(unsigned int index)
    {
        try
        {
            float dot_product = dotProduct(index);
            return acos(dot_product) * 180.0 / 3.1415926535897932384;
        }
        catch(std::exception *e)
        {
            throw e;
        }
    }

/*******************************************************************************
* MoveIn::ExtremePoints::boundaryPoint                                         *
*******************************************************************************/

    int ExtremePoints::boundaryPoint(unsigned char boundary,
                                     bool curvature)
    {
        if(size() == 0)
        {
            return -1;
        }
        CvPoint p;
        int index=-1;
        unsigned short int i;

        for(i=0; i<size(); i++)
        {
            p = getPoint(i);
            if(getCurvature(i) == curvature)
            {
                index = i;
                break;
            }
        }
        for(i=i+1; i<size(); i++)
        {
            if(getCurvature(i) == curvature)
            {
                if(boundary == LEFT_BOTTOM)
                {
                    if(getPoint(index).x > getPoint(i).x)
                    {
                        index = i;
                    }
                    else if(getPoint(index).x == getPoint(i).x)
                    {
                        if(getPoint(index).y < getPoint(i).y)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == LEFT_UP)
                {
                    if(getPoint(index).x > getPoint(i).x)
                    {
                        index = i;
                    }
                    else if(getPoint(index).x == getPoint(i).x)
                    {
                        if(getPoint(index).y > getPoint(i).y)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == RIGHT_BOTTOM)
                {
                    if(getPoint(index).x < getPoint(i).x)
                    {
                        index = i;
                    }
                    else if(getPoint(index).x == getPoint(i).x)
                    {
                        if(getPoint(index).y < getPoint(i).y)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == RIGHT_UP)
                {
                    if(getPoint(index).x < getPoint(i).x)
                    {
                        index = i;
                    }
                    else if(getPoint(index).x == getPoint(i).x)
                    {
                        if(getPoint(index).y > getPoint(i).y)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == BOTTOM_LEFT)
                {
                    if(getPoint(index).y < getPoint(i).y)
                    {
                        index = i;
                    }
                    else if(getPoint(index).y == getPoint(i).y)
                    {
                        if(getPoint(index).x > getPoint(i).x)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == BOTTOM_RIGHT)
                {
                    if(getPoint(index).y < getPoint(i).y)
                    {
                        index = i;
                    }
                    else if(getPoint(index).y == getPoint(i).y)
                    {
                        if(getPoint(index).x < getPoint(i).x)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == UPPER_LEFT)
                {
                    if(getPoint(index).y > getPoint(i).y)
                    {
                        index = i;
                    }
                    else if(getPoint(index).y == getPoint(i).y)
                    {
                        if(getPoint(index).x > getPoint(i).x)
                        {
                            index = i;
                        }
                    }
                }
                if(boundary == UPPER_RIGHT)
                {
                    if(getPoint(index).y > getPoint(i).y)
                    {
                        index = i;
                    }
                    else if(getPoint(index).y == getPoint(i).y)
                    {
                        if(getPoint(index).x < getPoint(i).x)
                        {
                            index = i;
                        }
                    }
                }
            }
        }
        return index;
    }

}

#endif



